/*
scanRegistration节点作用
1.接收激光雷达信息
2.计算曲率找出角点和平面点并发布出去
lidar被安装为x轴向前,y轴向左,z轴向上的右手坐标系

      | x                        | z
 y    |     ----->          x    | 
------Z                    ------Y 
scanRegistration会把两者通过交换坐标轴，都统一到z轴向前,x轴向左,y轴向上的右手坐标系
*/

#include<cmath>
#include<vector>
#include<ros/ros.h>
#include"common.h"
#include<iostream>
#include<opencv/cv.h>
#include<sensor_msgs/PointCloud2.h>
#include<sensor_msgs/Imu.h>
#include<nav_msgs/Odometry.h>
#include<pcl_conversions/pcl_conversions.h>
#include<std_msgs/String.h>
#include<pcl/point_types.h>
#include<pcl/point_cloud.h>
#include<pcl/filters/voxel_grid.h>
#include<pcl/kdtree/kdtree_flann.h>
#include<tf/transform_datatypes.h>
#include<tf/transform_broadcaster.h>

using namespace std;

//扫描周期
const double scanPeriod = 0.1;
//激光雷达线数
const int N_SCANS = 16;

//初始化控制变量
//弃用前20 帧数据
const int systemDelay = 20;
//丢弃帧数个数 >20就初始化
int systemInitCount = 0;
//是否初始化标志
bool systemInited = false;

//存储点云曲率
float cloudCurvature[40000];
//存储曲率点对应的序号
int cloudSortInd[40000];
//存储是否筛选过 选过为1 未选过为0
int cloudNeighborpicked[40000];
//点标记 1-代表曲率较大的点 0-代表曲率较小的点
int cloudLabel[40000];

//一帧点云信号发布者
ros::Publisher pubLaserCloud;
//边缘点发布者
ros::Publisher pubCornerPointsSharp;
//次边缘点发布者
ros::Publisher pubCornerPointsLessSharp;
//平面点发布者
ros::Publisher pubSurfPointsFlat;
//次平面点发布者
ros::Publisher pubSurfPointsLessFlat;




//接收点云信号的回调函数
void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
    //将前20个数据丢弃不处理
    if(!systemInited){
        systemInitCount++;
        if(systemInitCount >= systemDelay){
            systemInited = true;
        }
        return;
    }
    //一帧有16个scan
    //记录每个scan有曲率的点开始索引和结束索引
    //初始化为16个0
    std::vector<int> scanStartInd(N_SCANS,0);
    std::vector<int> scanEndIdx(N_SCANS,0);

    //当前点云时间
    double timeScanCur = laserCloudMsg->header.stamp.toSec();
    //输入点云
    pcl::PointCloud<pcl::PointXYZ> laserCloudIn;
    //将消息转换成pcl数据
    pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);
    //点索引
    std::vector<int> indices;
    //移除空点储存在laserCloudIn 并在indices存储点的索引
    pcl::removeNaNFromPointCloud(laserCloudIn,laserCloudIn,indices);
    //获取点云数量
    int cloudSize = laserCloudIn.points.size();
    //lidar开始点的旋转角 atan2范围[-pi,+pi],计算旋转角时取负号是因为velodyne是顺时针旋转 
    float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
    //lidar scan结束点的旋转角，加2*pi使点云旋转周期为2*pi
    float endOri = -atan2(laserCloudIn.points[cloudSize-1].y,
                          laserCloudIn.points[cloudSize-1].x) + 2*M_PI;
    //结束方位角与开始方位角差值控制在(PI,3*PI)范围，允许lidar不是一个圆周扫描
    //正常情况控制  pi < endOri - startOri < 3*pi，异常则修正
    if(endOri - startOri > 3 * M_PI){
        endOri -= 2 * M_PI;
    }                   
    if(endOri - startOri < M_PI){
        endOri += 2 * M_PI;
    }
    //雷达扫描是否旋转过半
    bool halfPassed = false;
    int count = cloudSize;
    PointType point;
    //按照16个线号存储点云的容器 
    std::vector< pcl::PointCloud<PointType> > laserCloudScans(N_SCANS);
    //把每个点按照线号放到容器中
    for (int i = 0 ; i <= cloudSize ; i++){
        //进行坐标轴转换
        point.x = laserCloudIn.points[i].y;
        point.z = laserCloudIn.points[i].x;
        point.y = laserCloudIn.points[i].z;
        //计算每个点的仰角来区分线号 两个线之间差2度
        float angle = atan(point.y/sqrt(point.x*point.x + point.z*point.z));
        //线号
        int scanID;
        //仰角四舍五入整数化 区分出线号
        int roundedAngle = int(angle + (angle<0.0?-0.5:+0.5));
        //大于0的线号为奇数 小于0的为偶数
        if(roundedAngle > 0){
            scanID = roundedAngle;
        }
        else{
            scanID = roundedAngle + (N_SCANS - 1);
        }
        //过滤点 只选[-15,+15]的点
        if(scanID > N_SCANS - 1 || scanID < 0 ){
            count--;
            continue;
        }
        //计算该点的旋转角
        float ori = -atan2(point.x, point.z);
        // //根据是否旋转过半判断从其实位置插值还是从结束为止插值
        // if(!halfPassed){

        // }
        // else{

        // }
        //判断startOri< ori < endOri 去除异常点
        if(ori > endOri){
            ori -= 2 * M_PI;
        }
        if(ori < startOri){
            ori += 2 *M_PI;
        }

        //如果还异常则删除 myself
        if(ori < startOri || ori > endOri){
            continue;
        }
        //计算该点的相对时间
        float relTime = (ori - startOri)/(endOri - startOri);
        point.intensity = scanID + scanPeriod * relTime;
        laserCloudScans[scanID].push_back(point);

    }

    //获得有效点的数量
    cloudSize = count;
    //该帧点云
    pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
    //将所有点按照线号放入到同一个容器
    for (size_t i = 0; i < N_SCANS; i++){
        *laserCloud += laserCloudScans[i];
    }
    //线数标志
    int scanCount = -1;
    //使用前后五个点计算每个点的曲率 每个线的前后5个都不要
    for (size_t i = 5; i < cloudSize - 5; i++){
        float diffX = laserCloud->points[i-5].x + laserCloud->points[i-4].x
                    + laserCloud->points[i-3].x + laserCloud->points[i-2].x
                    + laserCloud->points[i-1].x + laserCloud->points[i+1].x
                    + laserCloud->points[i+2].x + laserCloud->points[i+3].x
                    + laserCloud->points[i+4].x + laserCloud->points[i+5].x
                    -10 * laserCloud->points[i].x;
        float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y 
                    + laserCloud->points[i - 3].y + laserCloud->points[i - 2].y 
                    + laserCloud->points[i - 1].y + laserCloud->points[i + 5].y 
                    + laserCloud->points[i + 1].y + laserCloud->points[i + 2].y
                    + laserCloud->points[i + 3].y + laserCloud->points[i + 4].y
                    - 10 * laserCloud->points[i].y;
        float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z 
                    + laserCloud->points[i - 3].z + laserCloud->points[i - 2].z 
                    + laserCloud->points[i - 1].z + laserCloud->points[i + 5].z 
                    + laserCloud->points[i + 1].z + laserCloud->points[i + 2].z
                    + laserCloud->points[i + 3].z + laserCloud->points[i + 4].z
                    - 10 * laserCloud->points[i].z;

        //计算曲率
        cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;
        //记录曲率点的索引
        cloudSortInd[i] = i;
        //初始时，点全部未筛选
        cloudNeighborpicked[i] = 0;
        //初始化全部为 flat点
        cloudLabel[i] = 0;
        
        //每个scan的第一个点会进来
        if(int(laserCloud->points[i].intensity) != scanCount){
            //得到当前线数
            scanCount = int(laserCloud->points[i].intensity);
            //得到每个线的起始点与结束点
            if(scanCount > 0 && scanCount < N_SCANS){
                scanStartInd[scanCount] = i + 5;
                scanEndIdx[scanCount - 1] = i - 6;
            }
        }        
    }
    //第一个线开始点和最后一个线结束点
    scanStartInd[0] = 5;
    scanEndIdx[N_SCANS - 1] = cloudSize - 5;
    //挑选点 把容易被斜面挡住的点和离群点 以及与激光束平行的平面点
    for(int i = 0; i < cloudSize - 6; i++){
        //计算该点与后一个点的差值
        float diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x;
        float diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y;
        float diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z;
        //计算两点之间距离平方
        float diff = diffX * diffX + diffY * diffY + diffZ * diffZ;
        //排除容易被斜面挡住的点
        if(diff > 0.1){
            //该点的深度
            float depth1 = sqrt(laserCloud->points[i].x * laserCloud->points[i].x+
                                laserCloud->points[i].y * laserCloud->points[i].y+
                                laserCloud->points[i].z * laserCloud->points[i].z);
            //后一个点深度
            float depth2 = sqrt(laserCloud->points[i + 1].x * laserCloud->points[i + 1].x + 
                                laserCloud->points[i + 1].y * laserCloud->points[i + 1].y +
                                laserCloud->points[i + 1].z * laserCloud->points[i + 1].z);
            //按照两个点的深度比例，将深度较大的点拉回后计算角度
            if(depth1 > depth2){
                //将距离大的点拉回
                diffX = laserCloud->points[i+1].x - laserCloud->points[i].x * depth2/depth1;
                diffY = laserCloud->points[i+1].y - laserCloud->points[i].y * depth2/depth1;
                diffZ = laserCloud->points[i+1].z - laserCloud->points[i].z * depth2/depth1;

                //计算两条线的夹角
                if(sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ)/depth2 < 0.1){

                    //排除因斜面挡住的点 如果夹角比较小 那么排除该点之前的前五个点
                    cloudNeighborpicked[i-1] = 1;
                    cloudNeighborpicked[i-2] = 1;
                    cloudNeighborpicked[i-3] = 1;
                    cloudNeighborpicked[i-4] = 1;
                    cloudNeighborpicked[i-5] = 1;
                }

            }
            else{
                //将距离大的点拉回
                diffX = laserCloud->points[i].x - laserCloud->points[i+1].x * depth1/depth2;
                diffY = laserCloud->points[i].y - laserCloud->points[i+1].y * depth1/depth2;
                diffZ = laserCloud->points[i].z - laserCloud->points[i+1].z * depth1/depth2;

                //计算两条线的夹角
                if(sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ)/depth1 < 0.1){

                    //排除因斜面挡住的点 如果夹角比较小 那么排除该点后五个点
                    cloudNeighborpicked[i+1] = 1;
                    cloudNeighborpicked[i+2] = 1;
                    cloudNeighborpicked[i+3] = 1;
                    cloudNeighborpicked[i+4] = 1;
                    cloudNeighborpicked[i+5] = 1;
                    cloudNeighborpicked[i] = 1;
                }

            }
        }
        //计算该点与前一个点的距离
        float diffX2 = laserCloud->points[i].x - laserCloud->points[i - 1].x;
        float diffY2 = laserCloud->points[i].y - laserCloud->points[i - 1].y;
        float diffZ2 = laserCloud->points[i].z - laserCloud->points[i - 1].z;
        //与前一个点的距离平方和
        float diff2 = diffX2 * diffX2 + diffY2 * diffY2 + diffZ2 * diffZ2;

        //点深度的平方和
        float dis = laserCloud->points[i].x * laserCloud->points[i].x
                  + laserCloud->points[i].y * laserCloud->points[i].y
                  + laserCloud->points[i].z * laserCloud->points[i].z;
        //与前后点的平方和都大于深度平方和的万分之二，这些点视为离群点，包括陡斜面上的点，强烈凸凹点和空旷区域中的某些点，置为筛选过，弃用
        if (diff > 0.0002 * dis && diff2 > 0.0002 * dis) {
            cloudNeighborpicked[i] = 1;
        }

    }    
    //将每条线上的点分入相应的类别：边沿点和平面点
    pcl::PointCloud<PointType> cornerPointsSharp;
    pcl::PointCloud<PointType> cornerPointsLessSharp;
    pcl::PointCloud<PointType> surfPointsFlat;
    pcl::PointCloud<PointType> surfPointsLessFlat;
    //将每条线上的点分入相应的类别：边沿点和平面点
    for(int i = 0; i < N_SCANS; i++){
        //每个scan上的less flat点
        pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);
        //把每个scan分成六等份
        for(int j = 0; j < 6; j++){
            //六等份起点和终点
            int sp = scanStartInd[i] + (scanEndIdx[i] - scanStartInd[i]) * j / 6;
            int ep = scanStartInd[i] + (scanEndIdx[i] - scanStartInd[i]) *( j + 1 ) / 6 - 1;
            
            //按照曲率大小从小到大 选择排序 
            //排序后cloudSortInd按照曲率从小到达的顺序排列点
            //外层控制选的位置 
            for(int k = ep; k > sp; k--){
                //内层控制比较对象
                for(int l = sp; l < k - 1 ; l++){
                    if(cloudCurvature[l] > cloudCurvature[k]){
                        int temp = cloudSortInd[l];
                        cloudSortInd[l] = cloudSortInd[k];
                        cloudSortInd[k] = temp;
                    }
                }
            }
            //挑选每个分段曲率最大和比较大的点
            //挑选出的最大曲率的点个数
            int largestPickedNum = 0;
            for(int k = ep; k >= sp; k--){
                //点序 第一个为曲率最大的点
                int ind = cloudSortInd[k];
                //如果曲率大的点没被筛选过且曲率确实比较大
                if(cloudNeighborpicked[ind] == 0 && cloudCurvature[ind] > 0.1){
                    //最大曲率的点个数
                    largestPickedNum++;
                    //在每段上取两个最大曲率点作为CornerPointsSharp
                    if(largestPickedNum <= 2){
                        //2代表曲率很大
                        cloudLabel[ind] = 2;
                        cornerPointsSharp.push_back(laserCloud->points[ind]);
                        cornerPointsLessSharp.push_back(laserCloud->points[ind]);
                    }
                    //在每段上取20个比较大曲率点作为CornerPointsLessSharp
                    else if(largestPickedNum <= 20){
                        cornerPointsLessSharp.push_back(laserCloud->points[ind]);
                    }
                    else{
                        break;
                    }
                    //该点被筛选过了
                    cloudNeighborpicked[ind] = 1;

                    //防止特征点太密集，将前后五个点筛选出去 使得特征点分布更均匀
                    for(int l = 1; l <= 5; l++){
                        float diffX = laserCloud->points[ind+l].x
                                    - laserCloud->points[ind+l-1].x;
                        float diffY = laserCloud->points[ind+l].y
                                    - laserCloud->points[ind+l-1].y;
                        float diffZ = laserCloud->points[ind+l].z
                                    - laserCloud->points[ind+l-1].z;
                        if(diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05){
                            break;
                        }
                        cloudNeighborpicked[ind+l] = 1;
                    }
                    for(int l = -1; l >= -5; l--){
                        float diffX = laserCloud->points[ind+l].x
                                    - laserCloud->points[ind+l-1].x;
                        float diffY = laserCloud->points[ind+l].y
                                    - laserCloud->points[ind+l-1].y;
                        float diffZ = laserCloud->points[ind+l].z
                                    - laserCloud->points[ind+l-1].z;
                        if(diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05){
                            break;
                        }
                        cloudNeighborpicked[ind+l] = 1;
                    }                       
                }
            }
            //挑选出每个分段曲率小的点
            int smallestPickedNum = 0;
            for(int k = sp; k <= ep; k++){
                int ind = cloudSortInd[k];
                if(cloudNeighborpicked[ind] == 0 && cloudCurvature[ind] < 0.1){
                    //-1代表曲率很小的点
                    cloudLabel[ind] = -1;
                    surfPointsFlat.push_back(laserCloud->points[ind]);
                    smallestPickedNum++;
                    if(smallestPickedNum >= 4){
                        break;
                    }
                    cloudNeighborpicked[ind] = 1;

                    for(int l = 1; l <= 5; l++) {//同样防止特征点聚集
                        float diffX = laserCloud->points[ind + l].x 
                                    - laserCloud->points[ind + l - 1].x;
                        float diffY = laserCloud->points[ind + l].y 
                                    - laserCloud->points[ind + l - 1].y;
                        float diffZ = laserCloud->points[ind + l].z 
                                    - laserCloud->points[ind + l - 1].z;
                        if(diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {
                            break;
                        }
                        cloudNeighborpicked[ind + l] = 1;
                    }
                    for(int l = -1; l >= -5; l--){
                        float diffX = laserCloud->points[ind + l].x 
                                    - laserCloud->points[ind + l + 1].x;
                        float diffY = laserCloud->points[ind + l].y 
                                    - laserCloud->points[ind + l + 1].y;
                        float diffZ = laserCloud->points[ind + l].z 
                                    - laserCloud->points[ind + l + 1].z;
                        if(diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {
                            break;
                        }

                        cloudNeighborpicked[ind + l] = 1;
                    }
                }
            }
            //将剩余的点（包括之前被排除的点）全部归入平面点中less flat类别中
            for (int k = sp; k <= ep; k++) {
                if (cloudLabel[k] <= 0) {
                    surfPointsLessFlatScan->push_back(laserCloud->points[k]);
                }
            }

        }
        //由于less flat太多了 所以进行体素滤波
        pcl::PointCloud<PointType> surfPointsLessFlatScanDS;
        pcl::VoxelGrid<PointType> downSizeFilter;
        downSizeFilter.setInputCloud(surfPointsLessFlatScan);
        downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
        downSizeFilter.filter(surfPointsLessFlatScanDS);

        //less flat点汇总
        surfPointsFlat += surfPointsLessFlatScanDS;

    }
    //发布各种点
    //publish所有点
    sensor_msgs::PointCloud2 laserCloudOutMsg;
    pcl::toROSMsg(*laserCloud, laserCloudOutMsg);
    laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp;
    //设置点云坐标系名称
    laserCloudOutMsg.header.frame_id = "/camera";
    pubLaserCloud.publish(laserCloudMsg);
    
    //publich平面点和边沿点
    sensor_msgs::PointCloud2 cornerPointsSharpMsg;
    pcl::toROSMsg(cornerPointsSharp, cornerPointsSharpMsg);
    cornerPointsSharpMsg.header.stamp = laserCloudMsg->header.stamp;
    cornerPointsSharpMsg.header.frame_id = "/camera";
    pubCornerPointsSharp.publish(cornerPointsSharpMsg);

    sensor_msgs::PointCloud2 cornerPointsLessSharpMsg;
    pcl::toROSMsg(cornerPointsLessSharp, cornerPointsLessSharpMsg);
    cornerPointsLessSharpMsg.header.stamp = laserCloudMsg->header.stamp;
    cornerPointsLessSharpMsg.header.frame_id = "/camera";
    pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg);

    sensor_msgs::PointCloud2 surfPointsFlat2;
    pcl::toROSMsg(surfPointsFlat, surfPointsFlat2);
    surfPointsFlat2.header.stamp = laserCloudMsg->header.stamp;
    surfPointsFlat2.header.frame_id = "/camera";
    pubSurfPointsFlat.publish(surfPointsFlat2);

    sensor_msgs::PointCloud2 surfPointsLessFlat2;
    pcl::toROSMsg(surfPointsLessFlat, surfPointsLessFlat2);
    surfPointsLessFlat2.header.stamp = laserCloudMsg->header.stamp;
    surfPointsLessFlat2.header.frame_id = "/camera";
    pubSurfPointsLessFlat.publish(surfPointsLessFlat2);

}
// //接收IMU信号的回调函数
// void imuHandler(const sensor_msgs::Imu imuMsg){

// }

int main(int argc, char** argv){
    //初始化ROS节点
    ros::init(argc, argv, "scanRegistration");
    //节点句柄
    ros::NodeHandle nh;
    //订阅传感器传来的点运数据
    ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_points", 2 , laserCloudHandler);
    // //订阅IMU数据
    // ros::Subscriber subImu = nh.subscribe("/imu/data", 50, imuHandler);
    //发送一帧的点云数据
    pubLaserCloud = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_2", 2);
    //发送边缘点
    pubCornerPointsSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 2);
    //发送次边缘点
    pubCornerPointsLessSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 2);
    //发送平面点
    pubSurfPointsFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surf", 2);
    //发送次平面点
    pubSurfPointsLessFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_surf", 2);

    //消息队列
    //10HZ----->发送队列 ----->接收队列
    //                      回调函数执行
    //如果回调函数执行慢    接收队列就会有消息溢出
    
    //消息发布后放到消息池里，消息会作为回调函数参数，但不会马上执行 而是把callback函数放到了一个回调函数队列中
    //执行到spin()或者spinOnce()才会执行callback函数
    //ros::spin() 就一直循环执行callback函数队列 ros::spinOnce()只执行callback函数队列的第一个 然后继续往下执行
    //由于回调函数队列有限 如果spinOnce()执行的比较慢 消息发布速度比较快 就会导致队列溢出 一些回调函数被挤掉就不会被执行
    ros::spin();
    return 0;
    

}
